#include "MyRobot.h"

#define ABS(a) (a >= 0 ? a : -a)

/**
 * This code is run during the match after autonomous mode
 * is complete. This is where the majority of robot-running
 * code will occur. This file may get large when adding more
 * methods, and inherintly more complicated. Splitting this
 * page's methods into multiple pages will help keep the code
 * organized and easier to read, understand, debug, and update.
 */

void RobotDemo::OperatorControl()
{
	//Any initialization of stuff goes here, before the main
	//control loop begins.
	myRobot.SetSafetyEnabled(true);
	
	//Initialization of Driver Station
	dsLCD = DriverStationLCD::GetInstance();
	
	//Gear Shift
	bool isInFirstGear = true;
	bool hasShot = false;
	bool latchOpen = false;
	bool backDown = false, startDown = false;
	bool previousBackButtonState = false;

	compressor.Start();
	
	while (IsOperatorControl() && IsEnabled())
	{
		//this is causing one of the talon's to 'spaz', check into it, try swapping PWM cables to rule out
		//a bad talon (for whatever reason) or bad PWM cable, etc.
		myRobot.TankDrive((fabs(leftStick.GetY()) > JOYSTICKDEADSPACE ? leftStick.GetY() : 0.0),
						  (fabs(rightStick.GetY()) > JOYSTICKDEADSPACE ? rightStick.GetY() : 0.0));
	
		//if that doesn't work, uncomment this (and comment out the above) to rule out the code above as the issue.
		/*
		if ((leftStick.GetY() > JOYSTICKDEADSPACE || leftStick.GetY() < -JOYSTCKDEADSPACE) &&
			(rightStick.GetY() > JOYSTICKDEADSPACE || rightStick.GetY() < -JOYSTICKDEADSPACE))
			myRobot.TankDrive(leftStick.GetY(), rightStick.GetY());
		else if (leftStick.GetY() < JOYSTICKDEADSPACE && leftStick.GetY() > -JOYSTICKDEADSPACE &&
				(rightStick.GetY() > JOYSTICKDEADSPACE || rightStick.GetY() < -JOYSTICKDEADSPACE))
			myRobot.TankDrive(0.0, rightStick.GetY());
		else if ((leftStick.GetY() > JOYSTICKDEADSPACE || leftStick.GetY() < -JOYSTCKDEADSPACE) &&
				rightStick.GetY() < JOYSTICKDEADSPACE&& rightStick.GetY() > -JOYSTICKDEADSPACE)
			myRobot.TankDrive(leftStick.GetY(), 0.0);
		else myRobot.TankDrive(0.0, 0.0);
		
		
		//if it's still not working, try custom ABS function defined above to replicate the first one above
		
		myRobot.TankDrive((ABS(leftStick.GetY()) > JOYSTICKDEADSPACE ? leftStick.GetY() : 0.0),
						  (ABS(rightStick.GetY()) > JOYSTICKDEADSPACE ? rightStick.GetY() : 0.0));
		
		*/ 
		
		
		
		//LIGHT CODE AND COMPRESSOR DISPLAY
		if (compressor.GetPressureSwitchValue())
		{
			ledSpike.Set(Relay::kOn);
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Compressor Off");
			dsLCD->UpdateLCD();
		}
		else 
		{
			ledSpike.Set(Relay::kOff);
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Compressor On");
			dsLCD->UpdateLCD();
		}
		
		
		
		//LATCH
		if (xbox.GetRawButton(XBOX_BACK))
		{
			if (previousBackButtonState == false)
			{
				if (latchOpen == true) // && !armDown.Get()
				{
					latch.Set(false);
					retractLatch.Set(true);
					latchOpen = !latchOpen;
					//backDown = true;
					//Print to DS
					dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Back Pressed");
					dsLCD->UpdateLCD();
				}
				else if(latchOpen == false)
				{
					latch.Set(true);
					retractLatch.Set(false);
					latchOpen = !latchOpen;
					dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Back Released");
					dsLCD->UpdateLCD();
					//backDown = false;
				}
			}
			previousBackButtonState = true;
		}
		else
		{
			previousBackButtonState = false;
		}	
		
		//Emergency latch move
		if (xbox.GetRawButton(XBOX_B))
		{
			if (!backDown)
				{
					latch.Set(!latchOpen);
					retractLatch.Set(latchOpen);
					latchOpen = !latchOpen;
					backDown = true;
					//Print to DS
					dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "B Pressed");
					dsLCD->UpdateLCD();
				}
		}else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "B Released");
			dsLCD->UpdateLCD();
			backDown = false;
		}
		
		//SHOOTERS
		if (xbox.GetRawButton(XBOX_START))
		{
			if (!startDown)
			{
				launchers.Set(!hasShot);
				retractLaunchers.Set(hasShot);
				hasShot = !hasShot;
				startDown = true;
				//Print to DS
				dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Start Pressed");
				dsLCD->UpdateLCD();
			}
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Start Released");
			dsLCD->UpdateLCD();
			startDown = false;
		}
		
		//MORE SHIFTING
		if (rightStick.GetRawButton(2))
		{
			//FIRST GEAR
			if (!isInFirstGear)
			{
				shifters.Set(true);
				retractShifters.Set(false);
				isInFirstGear = true;
				//Print to DS
				dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "First Gear");
				dsLCD->UpdateLCD();
			}
		}
		
		//MORE SHIFTING
		if (rightStick.GetRawButton(3))
		{
			//SECOND GEAR
			if (isInFirstGear)
			{
				shifters.Set(false);
				retractShifters.Set(true);
				isInFirstGear = false;
				//Print to DS
				dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Second Gear");
				dsLCD->UpdateLCD();
			}
		}
		
		
		//PICKUP ARM
		if (xbox.GetRawButton(XBOX_Y))
		{
			if (!liftUp.Get()) pickUp.Set(0.50);
			else pickUp.Set(0.0);
		}
		else if (xbox.GetRawButton(XBOX_A))
		{
			if (liftDown.Get()) pickUp.Set(-0.50);
			else pickUp.Set(0.0);
		}
		else pickUp.Set(0.0);
		
		if (liftUp.Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Upper Limit");
			dsLCD->UpdateLCD();
		}
		if (!liftDown.Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Lower Limit");
			dsLCD->UpdateLCD();
		}
		
		// Capture Ball wheels
		if (xbox.GetRawButton(XBOX_LBUMP) || xbox.GetRawButton(XBOX_RBUMP))
		{
			captureBall.Set(-0.5);
		}
		else
		{
			captureBall.Set(0.0);
		}
		
		dsLCD->Clear();
		UpdateSmartDashboard();	//update the smart dashboard after everything is updated
		Wait(0.005); // wait for a motor update time
	}

}

